%% 适应度函数
% p_max 最大功率约束
% p_th 最大干扰功率约束
% 输入所有粒子，算出所有粒子的适应度？
% 还是说输入当前粒子，算出某一个粒子的适应度

function f = fitness_each(u, w, h, d_min)
    % 示例适应度函数：最大化通信速率 - 惩罚项

      sigma2 = db2pow(-10);    % 这里假设噪声功率也是给定的
      % 计算 SNR: Gamma
      Gamma = abs(h' * w)^2 / sigma2;
      % 计算通信速率 R
      R = log2(1 + Gamma);

    N = (length(u) - 2) / 2; % 发射天线个数
    t = reshape(u(1:2*N), [2, N]); % 发射天线坐标被reshape成了 [2 × N]的形式
    % 惩罚项，确保最小距离约束
    penalty = 0;
    for i = 1:N
        for j = i+1:N
            if norm(t(:,i) - t(:,j)) < d_min
                penalty = penalty + 100;
            end
        end
    end

    % 适应度值
    f = R - penalty;
    %fprintf('R: %d\n', R);
    %fprintf('penalty: %d\n', penalty);
end


% a = randn(2,10);
% size(a, 2);